#include "Quad4ShellCalculator.h"

#include <iostream>

#include "DataStructure/Input/Solver/DOFDirections.h"

#include "DBManager/IService/Input/Element/IElementService.h"
#include "DBManager/IService/Input/Material/IIsotropicMaterialService.h"
#include "DBManager/IService/Input/Node/INodeService.h"
#include "DBManager/IService/Input/Property/IPSHELLService.h"
#include "DBManager/IService/Input/Subcase/ISubcaseService.h"
#include "DBManager/IService/Result/IntegStress/IQUAD4IntegStressService.h"
#include "DBManager/IService/Result/Nodal/IDISPLACEMENTService.h"
#include "DBManager/IService/Result/ElementalForce/IQUAD4ForceService.h"

#include "SGFem/Pre/Assembly/Assembly.h"
#include "SGFem/Pre/Assembly/CoordinateProcessor.h"
#include "SGFem/Pre/Assembly/ModelInfoQuery.h"

#include "Operater.h"
#include "SGFem/ElementCalculator/Common/ElementTools.hpp"
#include "SGFem/ShellSecCalculator/ShellSectionParser.h"

using namespace std;
using namespace SG::Algebra;
using namespace SG::DataStructure::Common;
using SG::DataStructure::Common::PI;
using namespace SG::DataStructure::FEM;
using namespace SG::DBManager;
using SG::FEMSolver::AssemblyEigen;
using SG::FEMSolver::CoordinateProcessor;
using SG::FEMSolver::ModelInfoQuery;
using namespace SG::FEMSolver::Element;
using SG::FEMSolver::Element::ShellSectionParser;
using SG::DataStructure::Post::QUAD4ForceData;

/** @brief 将单元 刚度矩阵或者应力刚度矩阵从`单元局部坐标系`变换到`节点位移输出坐标系
 * `
 * @param T            3*3矩阵，   单元局部坐标系
 * @param points       单元节点在单元局部坐标系下的坐标值
 * @param dispCoordIds 单元的节点位移输出坐标系
 */
auto calcTransMatrix = [] (const SG::Algebra::Matrix& T, const std::vector<Point>& points, const IdArray& dispCoordIds) -> Matrixd
{
    Matrix Tele (24, 24);
    auto&  coordHandler = CoordinateProcessor::GetInstance ();
    for (std::size_t iNode = 0; iNode < 4; ++iNode)
    {
        const auto& cId = dispCoordIds[iNode];
        if (0 == cId)
        {
            Tele.BlockFill (iNode * 6, iNode * 6, T);
            Tele.BlockFill (iNode * 6 + 3, iNode * 6 + 3, T);
        }
        else
        {
            Matrix TDisp (3, 3);
            coordHandler.GetCoordTrans (dispCoordIds[iNode], points[iNode], _OUT TDisp);
            auto tmpT = TDisp * T;
            Tele.BlockFill (iNode * 6, iNode * 6, tmpT);
            Tele.BlockFill (iNode * 6 + 3, iNode * 6 + 3, tmpT);
        }
    }
    return Tele;
};

void Quad4ShellCalculator::Initialize (SG::DataStructure::FEM::ElementTypes type)
{
    m_type = type;
    m_shpFunc.Compute (m_integForm.GetPoints ());
    const SG::FEMSolver::Element::Gauss2D1X1P integForm0;
    m_shpFunc0.Compute (integForm0.GetPoints ());

    m_mem.Initialize ();
    m_plate.Initialize ();
}

/** @brief 单元刚度矩阵计算
 *
 * 分别调用膜单元和板单元计算器对刚度矩阵贡献。按照位置进行组装后将其旋转到全局坐标系下
 */
void Quad4ShellCalculator::ComputeStiffness (const CQUAD4Data&               cquad4,
                                             const SG::Algebra::Matrix&      coord,
                                             const ShellSecMaterialConstant& secConstant,
                                             const SG::Algebra::Matrix&      Tele,
                                             const SG::DataStructure::Common::Real&      zWarp,
                                             SG::Algebra::Matrix& _OUT       kg) const
{
    static const std::vector<std::size_t> indexM{ 0, 1, 5, 6, 7, 11, 12, 13, 17, 18, 19, 23 };
    static const std::vector<std::size_t> indexP{ 2, 3, 4, 8, 9, 10, 14, 15, 16, 20, 21, 22 };

    auto tp           = secConstant.m_t;
    Real thickness[4] = { tp, tp, tp, tp };

    // 变厚度标识
    bool variableT = cquad4.IsVariationalThickness ();
    if (secConstant.m_isComposite)
    {
        // 如果是复合材料， 将厚度全部改成 1.0
        for (int i = 0; i < 4; i++)
            thickness[i] = 1.0;
    }
    else if (variableT)
    {
        Real nodeThickness[4];
        cquad4.GetThickness (tp, nodeThickness);

        const auto nGauss = m_integForm.GetPointSize ();
        for (std::size_t iGauss = 0; iGauss < nGauss; ++iGauss)
        {
            auto& shp         = m_shpFunc.GetShapeFunction (iGauss);
            thickness[iGauss] = 0;
            for (int inode = 0; inode < 4; inode++)
            {
                thickness[iGauss] += nodeThickness[inode] * shp (inode, 0);
            }
        }
    }

    Matrix membraneKe (12, 12);   // 膜刚度
    Matrix plateKe (12, 12);      // 板刚度
    Matrix membPlateKe (12, 12);  // 拉弯刚度

    Matrix kmd (12, 4);
    Matrix kdd (4, 4);
    Matrix kpd (12, 4);
    Real   GV {0.0};

    Matrix Bb;
    Matrix Bs;

    Matrix Cm;
    Matrix Cb;
    Matrix Cs;

    auto JT0 = m_shpFunc0.GetShapeFunctionDerivative (0) * coord;

    Matrix     B, G, BmT;
    Real       j;

    const auto nGauss = m_integForm.GetPointSize ();
    for (std::size_t iGauss = 0; iGauss < nGauss; ++iGauss)
    {
        const Real&       integThickness = thickness[iGauss];
        const auto weight         = m_integForm.GetPoint (iGauss).m_weight;
        if (false == secConstant.m_purePlate)
        {
            // 膜协调部分
            m_mem.ComputeStrainMatrix (iGauss, coord, B, j);
            // Incompatible part
            m_mem.ComputeIncomStrainMatrix (iGauss, j, JT0, _OUT G);
            secConstant.GetCm (integThickness, _OUT Cm);  // 包含`厚度`

            const Real tmp = j * weight;
            BmT            = B.Transpose ();
            membraneKe += BmT * Cm * B * tmp;
            kmd += BmT * Cm * G * tmp;
            kdd += G.Transpose () * Cm * G * tmp;

            // 用于Drill刚度计算
            GV += tmp * Cm (2, 2);
        }

        // BmT, G 在计算耦合项使用
        if (!secConstant.m_pureMembrane)
        {
            secConstant.GetCb (integThickness, _OUT Cb);
            secConstant.GetCs (integThickness, _OUT Cs);

            // 板弯曲部分
            m_plate.ComputeBb (iGauss, coord, _OUT Bb, _OUT j);

            const auto& xi = m_integForm.GetPoint(iGauss).m_x;
            const auto& eta = m_integForm.GetPoint(iGauss).m_y;
            m_plate.ComputeBs (coord, j, xi, eta,_OUT Bs);
            const Real tmp = j * weight;
            plateKe += Bb.Transpose () * Cb * Bb * tmp;
            plateKe += Bs.Transpose () * Cs * Bs * tmp;

            if (secConstant.NeedCoupledItem ())
            {
                Matrix Cd;
                secConstant.GetCd (integThickness, _OUT Cd);  // 包含`厚度`

                kpd += Bb.Transpose () * Cd * G * tmp;

                membPlateKe -= BmT * Cd * Bb * tmp;
            }
        }
    }

    if (!secConstant.m_purePlate)
    {
        membraneKe -= (kmd * kdd.Inverse () * kmd.Transpose ());

        // drill
        const Real KROT6 = 100.0;

        const Real delta2{ 1.0e-4 * KROT6 };
        auto DN0Dx = JT0.Inverse () * m_shpFunc0.GetShapeFunctionDerivative (0);

        Matrix Q (1,
                  12,
                  {
                      -0.5 * DN0Dx (1, 0),
                      0.5 * DN0Dx (0, 0),
                      -0.25,
                      -0.5 * DN0Dx (1, 1),
                      0.5 * DN0Dx (0, 1),
                      -0.25,
                      -0.5 * DN0Dx (1, 2),
                      0.5 * DN0Dx (0, 2),
                      -0.25,
                      -0.5 * DN0Dx (1, 3),
                      0.5 * DN0Dx (0, 3),
                      -0.25,
                  });
        auto   tmp1 = GV * delta2;
        // auto tmpp=Q.Transpose() * (Q * tmp1);
        membraneKe += Q.Transpose () * (Q * tmp1);

        Matrix Q2 (1, 12, { 0, 0, 0.25, 0, 0, -0.25, 0, 0, 0.25, 0, 0, -0.25 });

        auto lambda = 1.0e-10 * GV * KROT6 / 4.0;
        // auto tmpppp = Q2.Transpose () * (Q2 * lambda);
        membraneKe += Q2.Transpose () * (Q2 * lambda);
    }

    if (false == secConstant.m_pureMembrane)
    {
        // condence
        plateKe -= (kpd * kdd.Inverse () * kpd.Transpose ());
        if (secConstant.NeedCoupledItem ())
            membPlateKe += (kmd * kdd.Inverse () * kpd.Transpose ());
    }

    // 组装单元刚度矩阵
    Matrix keLocal (24, 24);
    keLocal.BlockFill (indexM, indexM, membraneKe);
    keLocal.BlockFill (indexP, indexP, plateKe);

    if (secConstant.NeedCoupledItem ())
    {
        keLocal.BlockFill (indexM, indexP, membPlateKe);
        keLocal.BlockFill (indexP, indexM, membPlateKe.Transpose ());
    }

    // 处理 四点不共面翘曲
    if (fabs(zWarp) > 1.0e-6)
    {
        std::vector<Real> Zs{-zWarp, zWarp, -zWarp, zWarp};
        Matrix Tw = SG::Algebra::identity(24);
        for (size_t in = 0; in < 4; in++)
        {
            Tw (6 * in, (6 * in) + 4) = -Zs[in];
            Tw ((6 * in) + 1, (6 * in) + 3) = Zs[in];
        }

        keLocal = Tw.Transpose() * keLocal * Tw;
    }

    // 中面偏置处理
    const auto& zoff = cquad4.m_zoffs;
    if (fabs (zoff) > 1.0e-6)
    {
        auto        TOff = SG::Algebra::identity (6);
        TOff (0, 4)      = zoff;
        TOff (1, 3)      = -zoff;
        Matrix TToff (24, 24);
        TToff.BlockFill (0, 0, TOff);
        TToff.BlockFill (6, 6, TOff);
        TToff.BlockFill (12, 12, TOff);
        TToff.BlockFill (18, 18, TOff);
        keLocal = TToff.Transpose () * keLocal * TToff;
    }

    kg = Tele * keLocal * Tele.Transpose ();
}
/** @brief 单元质量矩阵计算
 *
 */
void Quad4ShellCalculator::ComputeMassMatrix (const SG::Algebra::Matrix& coord, const Real arealDensity, bool coupled, SG::Algebra::Matrix& _OUT MassMatrix) const
{
    // 积分前，将质量矩阵重置为0.0
    MassMatrix = Matrixd(24, 24);

    auto nGauss = m_integForm.GetPointSize ();
    SG::Algebra::Matrix Jaccob (2, 2);
    for (std::size_t i = 0; i != nGauss; ++i)
    {
        const auto& Nid = m_shpFunc.GetShapeFunctionDerivative (i);
        Jaccob          = Nid * coord;
        const Real detJ      = Jaccob (0, 0) * Jaccob (1, 1) - Jaccob (0, 1) * Jaccob (1, 0);

        const auto& Ni = m_shpFunc.GetShapeFunction (i);
        for (std::size_t j = 0; j != 4; ++j)
        {
            for (std::size_t k = 0; k != 4; ++k)
            {
                /** weight = 1.0 */
                const auto NiNj = Ni (j, 0) * Ni (k, 0) * detJ * arealDensity;
                MassMatrix (j * 6, k * 6) += NiNj;
                MassMatrix (j * 6 + 1, k * 6 + 1) += NiNj;
                MassMatrix (j * 6 + 2, k * 6 + 2) += NiNj;
            }
        }
    }

    if(!coupled)
    {
        MassMatrix = ElementTools::ComputeLumpMass (MassMatrix);
    }
}

/**
 * @brief   : 单元节点坐标，节点全局自由度索引，节点位移输出坐标系
 * @param   : nodeRepository 节点数据库
 * @param   : eleData          单元数据
 * @param   : points           节点坐标
 * @param   : globalIndex 节点位移自由度全局编号
 * @param   : dispCoordIds 位移输出坐标系变换
 * @author  : wanghuan
 * @date    : 2024.10.30
 */
void QuryElementInfo (const std::shared_ptr<SG::DBManager::INodeService> nodeRepository,
                      const CQUAD4Data&                                 eleData,
                      PointArray&                                       points,
                      std::vector<int>&                                 globalIndex,
                      std::vector<Id>&                                  dispCoordIds)
{
    std::vector<std::size_t> nodeIndexes (4);
    ModelInfoQuery::QueryNode (nodeRepository, eleData.m_g, 4, _OUT points, _OUT nodeIndexes, _OUT dispCoordIds);
    
    static const DofSet DOFs{ DOFDirections::DOF_Ux,   DOFDirections::DOF_Uy,   DOFDirections::DOF_Uz,
        DOFDirections::DOF_Rotx, DOFDirections::DOF_Roty, DOFDirections::DOF_Rotz };
    auto& assembly     = AssemblyEigen::GetInstance ();
    assembly.GetGlobalDOFId (nodeIndexes, DOFs, _OUT globalIndex);
}

/** @brief 板单元计算并保存刚度矩阵
 *
 */
void Quad4ShellCalculator::ComputeElementMatries (const SG::DBManager::DBServiceFactorySP dbServiceFactory)
{
    auto& assembly = AssemblyEigen::GetInstance ();

    // 壳属性计算器
    ShellSecMaterialConstant secConstant;
    ShellSectionParser       parser;


    CQUAD4Data                                     eleData;
    SG::DataStructure::FEM::PSHELLData             property;
    std::vector<Point>                             points (4);
    std::vector<int>                               globalIndex (24);
    std::vector<Id>                                dispCoordIds (4);
    Matrix                                         kGlobal (24, 24);
    Matrix                                         Mass (24, 24);

    const auto& eleRip         = dbServiceFactory->get<IElementService> ();
    const auto& nodeRepository = dbServiceFactory->get<INodeService> ();

    const auto eleSize = eleRip->GetElementSize (m_type);
    for (std::size_t index{ 0 }; index < eleSize; ++index)
    {
        eleRip->FindByIndex (index, _OUT eleData);
        QuryElementInfo (nodeRepository, eleData, _OUT points, _OUT globalIndex, _OUT dispCoordIds);

        // Local Coordinate system
        Matrix T;
        Matrix localCoord (4, 2);
        // 单元形心点
        Point eleCentral;
        ElementTools::computeQaud4CoordTrans (points, _OUT T, _OUT eleCentral, _OUT localCoord);

        //四节点不共面判断与确定媒介面偏置
        // Vector3D V3V1(points[2].m_x - points[0].m_x, points[2].m_y - points[0].m_y, points[2].m_z - points[0].m_z);
        // Vector3D V4V2(points[3].m_x - points[1].m_x, points[3].m_y - points[1].m_y, points[3].m_z - points[1].m_z);
        Vector3D V2V1(points[1].m_x - points[0].m_x, points[1].m_y - points[0].m_y, points[1].m_z - points[0].m_z);
        const Vector3D Vnormal{ T (0, 2), T (1, 2), T (2, 2) };
        const Real zWarp = dot(Vnormal, V2V1)/2.0;

        // G1-G2节点向量:
        vector<Real> vectorG1G2 = { localCoord (1, 0) - localCoord (0, 0), localCoord (1, 1) - localCoord (0, 1) };
        // 单元材料铺层方向
        vector<Real> eleMatAxis;
        Real         theta      = eleData.m_theta * PI / 180.0;
        bool transFlag = SG::FEMSolver::Element::calcMaterialOritation (eleData.m_mcId, eleCentral, T, vectorG1G2, theta, _OUT eleMatAxis);

        // Section Property,
        const auto pId    = eleData.m_pId;
        Status     status = parser.Compute (dbServiceFactory, pId, transFlag, eleMatAxis, _OUT secConstant);
        if (Status::STATUS_Failure == status)
        {
            assert (0 && "can't find section\n");
        }

        // 刚度矩阵从单元局部坐标系到 位移输出坐标系下的变换矩阵
        auto Tele = calcTransMatrix (T, points, dispCoordIds);

        ComputeStiffness (eleData, localCoord, secConstant, Tele, zWarp, _OUT kGlobal);
        assembly.AccumulateStiffness (globalIndex, kGlobal);

        // 质量矩阵
        const auto coupled = assembly.NeedCoupleMass ();
        Real RHOt = secConstant.GetArealDensity(secConstant.m_t);
        ComputeMassMatrix (localCoord, RHOt, coupled, _OUT Mass);
            // 中面偏置处理
        const auto& zoff = eleData.m_zoffs;
        if (fabs (zoff) > 1.0e-6)
        {
            auto        TOff = SG::Algebra::identity (6);
            TOff (0, 4)      = zoff;
            TOff (1, 3)      = -zoff;
            Matrix TToff (24, 24);
            TToff.BlockFill (0, 0, TOff);
            TToff.BlockFill (6, 6, TOff);
            TToff.BlockFill (12, 12, TOff);
            TToff.BlockFill (18, 18, TOff);
            Mass = TToff.Transpose () * Mass * TToff;
        }
        // Matrix Trans(24, 24);
        // for (int iNode = 0; iNode < 8; ++iNode)
        // {
        //     Trans.BlockFill (iNode * 3, iNode * 3, T);
        // }
        Mass = Tele.Transpose() * Mass * Tele;
        assembly.AccumulateCoupledMass (globalIndex, Mass);
    }
}

void Quad4ShellCalculator::ComputeinternalForces (const SG::Algebra::Matrix& memDisplacement, const SG::Algebra::Matrix& plateDisplacement, const SG::Algebra::Matrix& localCoord,  const SG::Algebra::Matrix& Cm, const SG::DataStructure::Common::Real* const thickness, SG::DataStructure::Post::QUAD4ForceData& _OUT internalForces)
{   
    //中面和上下表面应变
    Matrix midStrain (6, 4);
    Matrix topStrain (6, 4);
    Matrix botStrain (6, 4);

    Quad4ShellCalculator::ComputeIntegStrain (memDisplacement, plateDisplacement, thickness, Cm, localCoord, midStrain, topStrain, botStrain, false);

    // 目前缺少弹性矩阵功能，暂时以面内膜本构/厚度得到
    // 此方法仅用于各向同性材料
    Matrix elasticMatrix (6, 6);
    for (int i = 0; i < 2; i++)
        for (int j = 0; j < 2; j++)
            elasticMatrix[i][j] = Cm(i, j);  // / secConstant.m_t;

    elasticMatrix[3][3] = Cm(2, 2);          // / secConstant.m_t;
    elasticMatrix[4][4] = Cm(2, 2) * 5.0 / 6.0;
    elasticMatrix[5][5] = Cm(2, 2) * 5.0 / 6.0;

    auto midStress = elasticMatrix * midStrain;
    auto topStress = elasticMatrix * topStrain;
    auto botStress = elasticMatrix * botStrain;

    internalForces = QUAD4ForceData();
    //计算单元中心线力，采用4个积分点取平均
    for(std::size_t iGauss = 0; iGauss < 4; ++iGauss)
    {
        auto h = thickness[iGauss];
        internalForces.m_MX  += (topStress(0,iGauss)+4*midStress(0,iGauss)+botStress(0,iGauss))*h/6.0* 0.25;
        internalForces.m_MY  += (topStress(1,iGauss)+4*midStress(1,iGauss)+botStress(1,iGauss))*h/6.0* 0.25;
        internalForces.m_MXY += (topStress(3,iGauss)+4*midStress(3,iGauss)+botStress(3,iGauss))*h/6.0* 0.25;
        internalForces.m_BMX += (topStress(0,iGauss)-botStress(0,iGauss))*h*h/12.0* 0.25;
        internalForces.m_BMY += (topStress(1,iGauss)-botStress(1,iGauss))*h*h/12.0* 0.25;
        internalForces.m_BMXY+= (topStress(3,iGauss)-botStress(3,iGauss))*h*h/12.0* 0.25;
        internalForces.m_TX  += (topStress(4,iGauss)+4*midStress(4,iGauss)+botStress(4,iGauss))*h/6.0* 0.25;
        internalForces.m_TY  += (topStress(5,iGauss)+4*midStress(5,iGauss)+botStress(5,iGauss))*h/6.0* 0.25;
    }
}

void Quad4ShellCalculator::ComputeIntegStrain (const Matrix& memDisplacement, const Matrix& plateDisplacement, const Real* thickness, const Matrix& cm, const Matrix& localCoord, Matrix& _OUT strainM, Matrix& _OUT strainT,Matrix& _OUT strainB, bool onlyMid)
{
    m_mem.ComputeIntegStrain (memDisplacement, localCoord, cm, thickness, strainM);

    // 调用板单元计算
    const auto nGauss = m_integForm.GetPointSize ();
    for (std::size_t iGauss = 0; iGauss < nGauss; ++iGauss)
    {
        Matrix Bb, Bs;
        Real   jb;

        m_plate.ComputeBb (iGauss, localCoord, _OUT Bb, _OUT jb);
        // m_plate.ComputeBs (iGauss, jb, localCoord, _OUT Bs);
        auto xi = m_integForm.GetPoint(iGauss).m_x;
        auto eta = m_integForm.GetPoint(iGauss).m_y;
        m_plate.ComputeBs (localCoord, jb, xi, eta,_OUT Bs);

        // 计算板剪切应变
        for (int i = 0; i < 4; i++)
        {
            // 应变矩阵Bs顺序与常用排序相反，在此调整
            const auto posi = 3 * i;
            const auto w = plateDisplacement (posi, 0);
            const auto thetaX = plateDisplacement (posi + 1, 0);
            const auto thetaY = plateDisplacement (posi + 2, 0);
            strainM[5][iGauss] += Bs[0][posi] * w
                                            + Bs[0][posi + 1] * thetaX
                                            + Bs[0][posi + 2] * thetaY;
            strainM[4][iGauss] += Bs[1][posi] * w
                                            + Bs[1][posi + 1] * thetaX
                                            + Bs[1][posi + 2] * thetaY;
        }

        if (!onlyMid)
        {
            // 上下表面应变=中面应变+旋转角度*厚度/2
            //对6个应力分量进行遍历，将中面应变值赋给上下表面应变值
            for(int i = 0; i < 6; i++)
            {
                strainT[i][iGauss] = strainM[i][iGauss];
                strainB[i][iGauss] = strainM[i][iGauss]; 
            }
            
            //考虑由板弯曲引起的膜应变（计算上下表面）
            //考虑转角对上下表面应变的影响
            Real halfThk=0.5*thickness[iGauss];
            for(int i = 0; i < 4; i++)
            {
                const auto posi = 3 * i;
                const auto eBend11 = halfThk*(Bb[0][posi]* plateDisplacement (posi, 0)
                + Bb[0][posi + 1]* plateDisplacement (posi + 1, 0)
                + Bb[0][posi + 2]* plateDisplacement (posi + 2, 0));

                const auto eBend22 = halfThk*(Bb[1][posi]* plateDisplacement (posi, 0)
                + Bb[1][posi + 1]* plateDisplacement (posi + 1, 0)
                + Bb[1][posi + 2]* plateDisplacement (posi + 2, 0));

                const auto eBend12 = halfThk*(Bb[2][posi]* plateDisplacement (posi, 0)
                + Bb[2][posi + 1]* plateDisplacement (posi + 1, 0)
                + Bb[2][posi + 2]* plateDisplacement (posi + 2, 0));

                strainT[0][iGauss] += eBend11;
                strainT[1][iGauss] += eBend22;
                strainT[3][iGauss] += eBend12;

                strainB[0][iGauss] -= eBend11;
                strainB[1][iGauss] -= eBend22;
                strainB[3][iGauss] -= eBend12;                                                 
            }         
        }
    }
}

/** @brief 板单元计算单元局部坐标系下积分点应力
 *
 */
void Quad4ShellCalculator::DataRecovery (const SG::DBManager::DBServiceFactorySP             dbServiceFactory,
                                         int                                                 subcaseId, 
                                         SG::DataStructure::Common::Real time,
                                         const std::vector<SG::DataStructure::Common::Real>& globalDisplacement)
{
    CQUAD4Data         eleData;
    std::vector<int>   globalIndex (24);
    std::vector<Point> points (4);
    std::vector<Id>    dispCoordIds (4);

    // 单元节点全局坐标系位移向量
    Matrix eleDisplacement (24, 1);

    // 壳属性计算器
    ShellSecMaterialConstant secConstant;
    ShellSectionParser       parser;

    // 计算并保持单元 积分点应力
    auto para = dbServiceFactory->get<ISubcaseService> ()->FindSubCase (subcaseId);
    SG::DataStructure::FEM::Subcase subcase(para);
    const auto&    nodeRepository = dbServiceFactory->get<INodeService> ();
    // OutputSetting& outPutSetList  = subcase.m_out;
    if (1)
    {
        // 开辟空间按照索引存储所有 单元积分点应力
        std::vector<SG::Algebra::Matrixd> stressSet;
        // 单元膜单元局部坐标系下节点位移一维矩阵（向量）
        Matrix memDisplacement (12, 1);
        // 板单元局部坐标系下节点位移一维矩阵（向量）
        Matrix plateDisplacement (12, 1);
        const auto& eleRip = dbServiceFactory->get<IElementService> ();
        const auto eleSize = eleRip -> Size<SG::DataStructure::FEM::CQUAD4Data> ();
        //单元线力
        Matrix internalForces(8,1);
        QUAD4ForceData eleForceData;
        std::vector<QUAD4ForceData> eleForceDataSet (eleSize);
        for (std::size_t index{ 0 }; index < eleSize; ++index)
        {
            eleRip->FindByIndex (index, _OUT eleData);
            QuryElementInfo (nodeRepository, eleData, _OUT points, _OUT globalIndex, _OUT dispCoordIds);

            // 将单元全局坐标系下的节点信息转换到单元局部坐标系，存储于localCoord
            Matrix T;
            Matrix localCoord (4, 3);
            Point origin;
            ElementTools::computeQaud4CoordTrans (points, _OUT T, _OUT origin, _OUT localCoord);
            auto Tele = calcTransMatrix (T, points, dispCoordIds);

            // 单元局部坐标系下节点位移
            ElementTools::GetElementDisplacement (globalIndex, globalDisplacement, eleDisplacement);
            auto locDisp = Tele.Transpose () * eleDisplacement;

            // 为匹配膜、板弯曲、板剪切应变矩阵维度，将壳位移拆分成膜位移及板弯曲位移
            // 拆分单元位移, ToDo (wanghuan) 增加vector数据结构，增加或者子串的方法
            for (int i = 0; i < 4; i++)
            {
                memDisplacement (3 * i, 0)       = locDisp (6 * i, 0);
                memDisplacement (3 * i + 1, 0)   = locDisp (6 * i + 1, 0);
                memDisplacement (3 * i + 2, 0)   = locDisp (6 * i + 5, 0);
                plateDisplacement (3 * i, 0)     = locDisp (6 * i + 2, 0);
                plateDisplacement (3 * i + 1, 0) = locDisp (6 * i + 3, 0);
                plateDisplacement (3 * i + 2, 0) = locDisp (6 * i + 4, 0);
            }
            
            // TODO:  offset of reference plane

            // 单元形心点
            Point eleCentral{ (points[0].m_x + points[1].m_x + points[2].m_x + points[3].m_x) * 0.25,
                               (points[0].m_y + points[1].m_y + points[2].m_y + points[3].m_y) * 0.25,
                               (points[0].m_z + points[1].m_z + points[2].m_z + points[3].m_z) * 0.25 };  
            //G1-G2节点向量:
            vector<Real> vectorG1G2 = { localCoord (1, 0) - localCoord (0, 0), localCoord (1, 1) - localCoord (0, 1) };
            // 单元材料铺层方向
            vector<Real> eleMatAxis;
            Real         theta      = eleData.m_theta * PI / 180.0;
            bool transFlag = SG::FEMSolver::Element::calcMaterialOritation (eleData.m_mcId, eleCentral, T, vectorG1G2, theta, _OUT eleMatAxis);

            // Section Property,
            const auto pId    = eleData.m_pId;
            Status     status = parser.Compute (dbServiceFactory, pId, transFlag, eleMatAxis, _OUT secConstant);
            if (Status::STATUS_Failure == status)
            {
                assert (0 && "can't find section\n");
            }
            assert (secConstant.m_t > 1.0e-6);  // TODO. 无法处理通过单元数据指定厚度的情况 [wanghuan]

            // 重复刚度矩阵计算积分点厚度计算过程
            // 后续可考虑将四积分点厚度存储以节省计算时间 lxy
            auto tp           = secConstant.m_t;
            Real thickness[4] = { tp, tp, tp, tp };

            // 变厚度标识
            bool variableT = eleData.IsVariationalThickness ();
            if (variableT)
            {
                Real nodeThickness[4];
                eleData.GetThickness (tp, nodeThickness);

                const auto nGauss = m_integForm.GetPointSize ();
                for (std::size_t iGauss = 0; iGauss < nGauss; ++iGauss)
                {
                    auto& shp         = m_shpFunc.GetShapeFunction (iGauss);
                    thickness[iGauss] = 0;
                    for (int inode = 0; inode < 4; inode++)
                    {
                        thickness[iGauss] += nodeThickness[inode] * shp (inode, 0);
                    }
                }
            }

            Matrix cm;
            secConstant.GetCm(1.0, cm);
            // 应变矩阵行为分量依次为：xx,yy,zz,xy,yz,xz，列为积分点
            //中面应变
            Matrix eleMidIntegStrain (6, 4);
            //上下表面应变:
            Matrix eleTopIntegStrain (6, 4);
            Matrix eleBotIntegStrain (6, 4);
            // 仅仅计算中面
            this -> ComputeIntegStrain (memDisplacement, plateDisplacement, thickness, cm, localCoord, eleMidIntegStrain, eleTopIntegStrain, eleBotIntegStrain);

            // 目前缺少弹性矩阵功能，暂时以面内膜本构/厚度得到
            // 此方法仅用于各向同性材料
            Matrix elasticMatrix (6, 6);
            for (int i = 0; i < 2; i++)
                for (int j = 0; j < 2; j++)
                    elasticMatrix[i][j] = secConstant.m_Cm[i][j];  // / secConstant.m_t;

            elasticMatrix[3][3] = secConstant.m_Cm[2][2];          // / secConstant.m_t;
            elasticMatrix[4][4] = secConstant.m_Cm[2][2] * 5.0 / 6.0;
            elasticMatrix[5][5] = secConstant.m_Cm[2][2] * 5.0 / 6.0;

            const auto integMidStress = elasticMatrix * eleMidIntegStrain;
            stressSet.push_back (integMidStress);

            // 计算单元内力
            ComputeinternalForces (memDisplacement, plateDisplacement, localCoord, cm, thickness, eleForceData);
            eleForceData.m_eId  = eleData.m_id;

            eleForceDataSet[index] = eleForceData;

        }
        // 积分点应力保存完毕
        dbServiceFactory->get<IQUAD4IntegStressService> ()->AddBySubcaseId (subcaseId, stressSet);
        // 内力保存完毕
        dbServiceFactory->get<IQUAD4ForceService> ()->AddBySubcaseId (subcaseId, eleForceDataSet);
    }
}

// 计算全局坐标系下单元应力刚度矩阵
void Quad4ShellCalculator::ComputeStressStiffness (const SG::DBManager::DBServiceFactorySP dbServiceFactory, int staticSubId)
{
    CQUAD4Data         eleData;
    std::vector<int>   globalIndex (24);
    std::vector<Point> points (4);
    std::vector<Id>    dispCoordIds (4);

    // 壳属性计算器
    ShellSectionParser       parser;
    ShellSecMaterialConstant secConstant;

    auto&       assembly       = AssemblyEigen::GetInstance ();
    const auto& nodeRepository = dbServiceFactory->get<INodeService> ();

    // 从数据库提取积分点应力结果
    std::vector<SG::Algebra::Matrixd> stressSet;
    dbServiceFactory->get<IQUAD4IntegStressService> ()->FindBySubcaseId (staticSubId, _OUT stressSet);
    const auto& eleRip = dbServiceFactory->get<IElementService> ();
    const auto eleSize = eleRip -> Size<SG::DataStructure::FEM::CQUAD4Data> ();

    for (std::size_t iEle{ 0 }; iEle < eleSize; ++iEle)
    {
        eleRip->FindByIndex (iEle, _OUT eleData);
        QuryElementInfo (nodeRepository, eleData, _OUT points, _OUT globalIndex, _OUT dispCoordIds);

        // 单元局部坐标系， 及相应的节点局部坐标
        Matrix T;
        Matrix localCoord (4, 3);
        Point origin;
        ElementTools::computeQaud4CoordTrans (points, _OUT T, _OUT origin, _OUT localCoord);

        auto Tele = calcTransMatrix (T, points, dispCoordIds);

        // 单元坐标系下应力刚度矩阵
        Matrix kSigma (24, 24);
        // 导数矩阵G
        Matrix G (10, 24);
        // 应力矩阵S
        Matrix S (10, 10);

        // 查询积分点厚度
        const auto pId    = eleData.m_pId;
        //获得截面属性(积分点厚度)不涉及材料转换调用简化版Compute
        Status     status = parser.Compute (dbServiceFactory, pId, _OUT secConstant);

        //Status     status = parser.Compute (dbServiceFactory, pId, transFlag, eleMatAxis, _OUT secConstant);

        if (Status::STATUS_Failure == status)
        {
            assert (0 && "can't find section\n");
        }
        assert (secConstant.m_t > 1.0e-6);  // TODO. 无法处理通过单元数据指定厚度的情况 [wanghuan]
        // 获取厚度
        const auto& t = secConstant.m_t;


        // 重复刚度矩阵计算积分点厚度计算过程
        // 后续可考虑将四积分点厚度存储以节省计算时间 lxy
        Real thickness[4] = { t, t, t, t };

        // 变厚度标识
        bool variableT = eleData.IsVariationalThickness ();
        if (variableT)
        {
            Real nodeThickness[4];
            eleData.GetThickness (t, nodeThickness);

            const auto nGauss = m_integForm.GetPointSize ();
            for (std::size_t iGauss = 0; iGauss < nGauss; ++iGauss)
            {
                auto& shp         = m_shpFunc.GetShapeFunction (iGauss);
                thickness[iGauss] = 0;
                for (int inode = 0; inode < 4; inode++)
                {
                    thickness[iGauss] += nodeThickness[inode] * shp (inode, 0);
                }
            }
        }
        
        // 从数据库取单元积分点应力
        const Matrix& eleIntegStress = stressSet[iEle];

        const auto nGauss = m_integForm.GetPointSize ();
        for (std::size_t iGauss = 0; iGauss < nGauss; ++iGauss)
        {
            
            //获得积分点厚度
            const Real&       integThickness = thickness[iGauss];

            // 计算自然坐标系下型函数导数
            const auto& DNDxi = m_shpFunc.GetShapeFunctionDerivative (iGauss);
            // 计算雅各比矩阵及其行列式
            auto JT  = DNDxi * localCoord;
            auto det = JT (0, 0) * JT (1, 1) - JT (0, 1) * JT (1, 0);
            // 计算单元局部坐标系下形函数导数
            Matrix DMDx = JT.Inverse () * DNDxi;

            // 组装G矩阵
            for (int i = 0; i < 4; i++)
            {
                for (int j = 0; j < 2; j++)
                {
                    double sigtmp        = DMDx[j][i];
                    int    rowLoc        = 6 * i;
                    G[j][rowLoc]         = sigtmp;
                    G[j + 2][rowLoc + 1] = sigtmp;
                    G[j + 4][rowLoc + 2] = sigtmp;
                    G[j + 6][rowLoc + 3] = sigtmp;
                    G[j + 8][rowLoc + 4] = sigtmp;
                }
            }

            // 取应力分量
            auto sigmaX  = eleIntegStress (0, iGauss);  // 应力x分量
            auto sigmaY  = eleIntegStress (1, iGauss);  // 应力y分量
            auto sigmaXY = eleIntegStress (2, iGauss);  // 应力xy分量
            // 组装S矩阵
            for (int i = 0; i < 3; i++)
            {
                S[2 * i][2 * i]         = sigmaX * integThickness;
                S[2 * i + 1][2 * i + 1] = sigmaY * integThickness;
                S[2 * i][2 * i + 1]     = sigmaXY * integThickness;
                S[2 * i + 1][2 * i]     = sigmaXY * integThickness;
            }

            for (int i = 3; i < 5; i++)
            {
                S[2 * i][2 * i]         = sigmaX * integThickness * integThickness * integThickness / 12;
                S[2 * i + 1][2 * i + 1] = sigmaY * integThickness * integThickness * integThickness / 12;
                S[2 * i][2 * i + 1]     = sigmaXY * integThickness * integThickness * integThickness / 12;
                S[2 * i + 1][2 * i]     = sigmaXY * integThickness * integThickness * integThickness / 12;
            }

            auto GT = G.Transpose ();

            kSigma += GT * S * G * (det * m_integForm.GetPoint (iGauss).m_weight);
        }

        // 应力刚度矩阵变换到全局坐标系
        kSigma = Tele * kSigma * Tele.Transpose ();
        assembly.AccumulateStressStiffness (globalIndex, kSigma);
    }
}

